#include"TShieldGenerator.h"



/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorBase */

void TShieldGeneratorBase::getXMap(int *xMap, TVarListHandler *xSupport) {
	int x;
	for(x=0;x<xSupport->res;x++) {
		xMap[x]=xSupport->varList[x].at(0);
	}
}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorGrid_SqrEuclidean */



TShieldGeneratorGrid_SqrEuclidean::TShieldGeneratorGrid_SqrEuclidean(int _dim, int *_xDimH, int *_yDimH, int _nLayers) {
	dim=_dim;
	xDimH=_xDimH;
	yDimH=_yDimH;
	nLayers=_nLayers;
	
	xStrides=(int*) malloc(sizeof(int)*dim);
	yStrides=(int*) malloc(sizeof(int)*dim);

	setLayer(0);
}

TShieldGeneratorGrid_SqrEuclidean::~TShieldGeneratorGrid_SqrEuclidean() {
	free(xStrides);
	free(yStrides);
}


int TShieldGeneratorGrid_SqrEuclidean::setLayer(int newLayer) {
	if(newLayer>=nLayers) {
		return ERR_MULTISCALE_EXCEEDEDLEVELS;
	}
	layer=newLayer;
	xDims=xDimH+dim*layer;
	yDims=yDimH+dim*layer;
	GridToolsGetStrides(dim,xDims,xStrides);
	GridToolsGetStrides(dim,yDims,yStrides);

	return 0;
}


void TShieldGeneratorGrid_SqrEuclidean::generateShield(TVarListHandler *xVars, TVarListHandler *xSupport) {
	int *xPos,d;
	int *xMap;
	// A shield is generated by iterating over all x-variables.
	// For convenience this iteration is not simply carried out 1..xres
	// But in grid coordinates directly. Then the existence, location and number of neighbours can be
	// determined more directly.


	/* extract map from support */
	xMap=(int*) malloc(sizeof(int)*xSupport->res);
	getXMap(xMap,xSupport);

	// xPos will keep the current coordinates on the grid
	xPos=(int*) malloc(sizeof(int)*dim);
	for(d=0;d<dim;d++) {
		xPos[d]=0;
	}

	// start iterating along the first axis
	iterateXVariables(xVars, xMap, xPos, 0);

	free(xMap);
	free(xPos);

}

void TShieldGeneratorGrid_SqrEuclidean::iterateXVariables(TVarListHandler *xVars, int *xMap, int *xPos, int d) {
	int x;
	if(d<dim) {
		// as long as we have not reached the last axis, launch iteration along next axis
		for(x=0;x<xDims[d];x++) {
			xPos[d]=x;
			iterateXVariables(xVars,xMap,xPos,d+1);
		}
	} else {
		// once a specific grid point is reached, add points
		addVariables_Shields(xVars,xMap,xPos);
		addVariables_Rectangles(xVars,xMap,xPos);
	}
}

void TShieldGeneratorGrid_SqrEuclidean::addVariables_Shields(TVarListHandler *xVars, int *xMap, int *xPos) {
	int xId,d;
	xId=GridToolsGetIdFromPos(dim,xPos,xStrides);
	for(d=0;d<dim;d++) {
		if(xPos[d]>0) {
			xVars->addToLine(xId,xMap[xId-xStrides[d]]);
		}
		if(xPos[d]<xDims[d]-1) {
			xVars->addToLine(xId,xMap[xId+xStrides[d]]);
		}
	}
}

void TShieldGeneratorGrid_SqrEuclidean::addVariables_Rectangles(TVarListHandler *xVars, int *xMap, int *xPos) {
	int *yPos,*yMin,*yMax,d,yId,xId;

	yPos=(int*) malloc(sizeof(int)*dim);
	yMin=(int*) malloc(sizeof(int)*dim);
	yMax=(int*) malloc(sizeof(int)*dim);
	for(d=0;d<dim;d++) {
		yPos[d]=0;
		yMin[d]=0;
		yMax[d]=0;
	}

	// determine dimensions of cube on Y grid
	// first determine id of current xPos
	xId=GridToolsGetIdFromPos(dim,xPos,xStrides);
	// go through all dimensions
	for(d=0;d<dim;d++) {
		// along each dimension, check whether we're on lower boundary
		if(xPos[d]>0) {
			// if not, determine yId of lower neighbour
			yId=xMap[xId-xStrides[d]];
			// compute Pos variable
			GridToolsGetPosFromId(dim,yId,yPos,yStrides);
			// set yMin[d] to corresponding entry
			yMin[d]=yPos[d];
		} else {
			// if x is at lower boundary, all y need to be added
			yMin[d]=0;
		}
		// now same procedure for upper boundary
		if(xPos[d]<xDims[d]-1) {
			yId=xMap[xId+xStrides[d]];
			GridToolsGetPosFromId(dim,yId,yPos,yStrides);
			// add +1 here for later for loop compatibility
			yMax[d]=yPos[d]+1;
		} else {
			yMax[d]=yDims[d];
		}
	}

	// start iterating along the first axis
	iterateYVariables(xVars, xId,yPos,yMin,yMax, 0);

	free(yPos);
	free(yMin);
	free(yMax);
}


void TShieldGeneratorGrid_SqrEuclidean::iterateYVariables(TVarListHandler *xVars, int xId,
		int *yPos, int *yMin, int *yMax, int d) {
	int y;
	if(d<dim) {
		// as long as we have not reached the last axis, launch iteration along next axis
		for(y=yMin[d];y<yMax[d];y++) {
			yPos[d]=y;
			iterateYVariables(xVars,xId,yPos,yMin,yMax,d+1);
		}
	} else {
		int yId;
		yId=GridToolsGetIdFromPos(dim,yPos,yStrides);
		//cout << yId << " ";
		xVars->addToLine(xId,yId);

	}
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorTreeBase */


TShieldGeneratorTreeBase::TShieldGeneratorTreeBase(int _dim,
		THierarchicalPartition* _yPartition, double** _yPos, double** _yRadii,
		int _lBottom, int _lTop,
		int *_xresH, double **_xposH, TVarListHandler **_xNeighboursH) {


	dim=_dim;
	yPartition=_yPartition;
	yPos=_yPos;
	yRadii=_yRadii;
	lTop=_lTop;
	
	xresH=_xresH;
	xposH=_xposH;
	xNeighboursH=_xNeighboursH;

	nLayers=yPartition->nLayers;
	
	setLayer(_lBottom);
}

TShieldGeneratorTreeBase::~TShieldGeneratorTreeBase() {
}


int TShieldGeneratorTreeBase::setLayer(int newLayer) {
	if(newLayer>=nLayers) {
		return ERR_MULTISCALE_EXCEEDEDLEVELS;
	}
	lBottom=newLayer;
	xpos=xposH[lBottom];
	xNeighbours=xNeighboursH[lBottom];
	xres=xresH[lBottom];

	return 0;

}


void TShieldGeneratorTreeBase::generateShield(TVarListHandler *xVars, TVarListHandler *xSupport)
 {
	int x;
	int *xMap;

	/* extract map from support */
	xMap=(int*) malloc(sizeof(int)*xSupport->res);
	getXMap(xMap,xSupport);


	for(x=0;x<xres;x++) {
		addVariables_Shields(xVars, xMap, x);
		addVariables_Polytopes(xVars, xMap, x);
	}

	free(xMap);

}

void TShieldGeneratorTreeBase::addVariables_Shields(
		TVarListHandler* xVars, int* xMap, int x) {
	int nIndex,xn;
	// iterate over neighbours of x
	for(nIndex=0;nIndex<xNeighbours->lenList[x];nIndex++) {
		// extract id of neighbour
		xn=xNeighbours->varList[x][nIndex];
		//cout << "\t\t" << xn << endl;
		xVars->addToLine(x,xMap[xn]);
	}
}

void TShieldGeneratorTreeBase::addVariables_Polytopes(
		TVarListHandler* xVars, int* xMap, int x) {
	int yBParent;
	for(yBParent=0;yBParent<yPartition->layers[lTop]->nCells;yBParent++) {
		iterateYVariables(xVars,xMap,x,lTop,yBParent);
	}
}

void TShieldGeneratorTreeBase::iterateYVariables(TVarListHandler* xVars,
		int* xMap, int xA, int lParent, int yBParent) {

	int yB,yBIndex;
	// this function is to be called only for coarser layers, not on finest layer.

	// for convenience, introduce direct references to relevant layer
	TPartitionLayer *parentLayer;
	parentLayer=yPartition->layers[lParent];
	// go through all children of current node
	for(yBIndex=0;yBIndex<parentLayer->nChildren[yBParent];yBIndex++) {
		// extract id
		yB=parentLayer->children[yBParent][yBIndex];
		// call hierarchical shielding condition test
		if(!checkCondition(xA,lParent+1,yB,xMap)) {
			// if condition fails
			if(lParent<lBottom-1) {
				// do test at finer scale
				iterateYVariables(xVars,xMap,xA,lParent+1,yB);
			} else {
				// when on finest scale, add variable
				xVars->addToLine(xA,yB);
			}
		}
	}

}


bool TShieldGeneratorTreeBase::checkCondition(int xA, int l, int yB,
		int* xMap) {

	int nIndex,xn,yn;

	//nCalls_checkCondition[l-lTop]++;

	for(nIndex=0;nIndex<xNeighbours->lenList[xA];nIndex++) {
		// extract id of neighbour
		xn=xNeighbours->varList[xA][nIndex];
		yn=xMap[xn];
		//nCalls_checkConditionPlane[l-lTop]++;
		if(checkConditionPlane(xA,xn,l,yB,yn)) {
			//cout << "\t\t(" << l << "," << yB << ") : true" << endl;
			return true;
		}
	}

	//cout << "\t\t(" << l << "," << yB << ") : false" << endl;
	return false;
}

bool TShieldGeneratorTreeBase::checkConditionPlane(__attribute__((unused)) int xA,
		__attribute__((unused)) int x, __attribute__((unused)) int l,
		__attribute__((unused)) int yB, __attribute__((unused)) int y) {
	return false;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorTree_SqrEuclidean */

template<class TReferenceTree>
TShieldGeneratorTree_SqrEuclideanPrototype<TReferenceTree>::TShieldGeneratorTree_SqrEuclideanPrototype(int _dim,
		THierarchicalPartition* _yPartition, double** _yPos, double** _yRadii,
		int _lBottom, int _lTop,
		int *_xresH, double **_xposH, TVarListHandler **_xNeighboursH) :
				TReferenceTree(_dim, _yPartition, _yPos, _yRadii,
						_lBottom, _lTop,
						_xresH, _xposH, _xNeighboursH) {
}



template<class TReferenceTree>
bool TShieldGeneratorTree_SqrEuclideanPrototype<TReferenceTree>::checkConditionPlane(int xA, int x, int l,
		int yB, int y) {
	// xA: index of initial x
	// yB: index of final y
	// x,y: indices of shielding pair
	// l: hierarchy level. the higher, the finer the resolution.
	double dist,norm,xpart;
	int i;
	// xpart: x-xA (called a in notes)
	// dist: (yB-y).(x-xA)
	// norm: in the end: length of x-xA (sqrt only taken in final else statement)
	dist=0;
	norm=0;
	for(i=0;i<dim;i++) {
		xpart=(xpos[x*dim+i]-xpos[xA*dim+i]);
		dist+=xpart*(yPos[l][yB*dim+i]-yPos[lBottom][y*dim+i]);
		norm+=xpart*xpart;
	}
	if(l>=lBottom) {
		return (dist>shieldingTolerance);
	} else {
		norm=sqrt(norm);
		return ((dist-norm*yRadii[l][yB])>shieldingTolerance);
	}
}


/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorTree_SqrEuclideanNoise */


template<class TReferenceTree>
TShieldGeneratorTree_SqrEuclideanNoisePrototype<TReferenceTree>::TShieldGeneratorTree_SqrEuclideanNoisePrototype(int _dim,
		THierarchicalPartition* _yPartition, double** _yPos, double** _yRadii,
		int _lBottom, int _lTop,
		int *_xresH, double **_xposH, TVarListHandler **_xNeighboursH,
		double **_c, double _eta, double _lambda) :
			TReferenceTree(_dim, _yPartition, _yPos, _yRadii,
						_lBottom, _lTop,
						_xresH, _xposH, _xNeighboursH) {
	c=_c;
	eta=_eta;
	lambda=_lambda;
	yres=_yPartition->layers[_lBottom]->nCells;
}


template<class TReferenceTree>
bool TShieldGeneratorTree_SqrEuclideanNoisePrototype<TReferenceTree>::checkConditionPlane(int xA, int x, int l,
		int yB, int y) {
	// xA: index of initial x
	// yB: index of final y
	// x,y: indices of shielding pair
	// l: hierarchy level. the higher, the finer the resolution.
	double dist,norm,xpart;
	int i;
	// xpart: x-xA (called a in notes)
	// dist: (yB-y).(x-xA)
	// norm: in the end: length of x-xA (sqrt only taken in final else statement)
	//EUCL_lincomb(xpos+(x*dim),xpos+(xA*dim),xpart,1.,-1.,dim);


	if(l>=lBottom) {
		dist=c[l][xA*yres+yB]-c[l][x*yres+yB]-c[l][xA*yres+y]+c[l][x*yres+y];
		return (dist>shieldingTolerance);
	} else {
		dist=0;
		norm=0;
		for(i=0;i<dim;i++) {
			xpart=(xpos[x*dim+i]-xpos[xA*dim+i]);
			dist+=xpart*(yPos[l][yB*dim+i]-yPos[lBottom][y*dim+i]);
			norm+=xpart*xpart;
		}

		norm=sqrt(norm);
		return ((dist-norm*yRadii[l][yB])-eta-lambda*norm>shieldingTolerance);
	}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* TShieldGeneratorTree_PEuclidean */

template<class TReferenceTree>
TShieldGeneratorTree_PEuclideanPrototype<TReferenceTree>::TShieldGeneratorTree_PEuclideanPrototype(int _dim,
		THierarchicalPartition* _yPartition, double** _yPos, double** _yRadii,
		int _lBottom, int _lTop,
		int *_xresH, double **_xposH, TVarListHandler **_xNeighboursH,
		double _p, double _slack) :
				TReferenceTree(_dim, _yPartition, _yPos, _yRadii,
						_lBottom, _lTop,
						_xresH, _xposH, _xNeighboursH) {



	p=_p;
	slack=_slack;
	xAyB=(double*) malloc(sizeof(double)*dim);
	xAys=(double*) malloc(sizeof(double)*dim);
	xsyB=(double*) malloc(sizeof(double)*dim);
	xsys=(double*) malloc(sizeof(double)*dim);
	xAxs=(double*) malloc(sizeof(double)*dim);
}

template<class TReferenceTree>
TShieldGeneratorTree_PEuclideanPrototype<TReferenceTree>::~TShieldGeneratorTree_PEuclideanPrototype() {
	free(xAyB);
	free(xAys);
	free(xsyB);
	free(xsys);
	free(xAxs);
}

template<class TReferenceTree>
bool TShieldGeneratorTree_PEuclideanPrototype<TReferenceTree>::checkConditionPlane(int xA, int x, int l,
		int yB, int y) {
	// xA: index of initial x
	// yB: index of final y
	// x,y: indices of shielding pair
	// l: hierarchy level. the higher, the finer the resolution.
	double dist;


	// compute necessary vector differences
	EUCL_lincomb(xpos+(xA*dim),yPos[lBottom]+(y*dim),xAys,1.,-1.,dim); // xA-ys
	EUCL_lincomb(xpos+(x*dim),yPos[lBottom]+(y*dim),xsys,1.,-1.,dim); // xs-ys
	EUCL_lincomb(xpos+(x*dim),yPos[l]+(yB*dim),xsyB,1.,-1.,dim); // xs-yB

	// compute exact r.h.s. of shielding condition
	dist=-(getPhi(xAys)-getPhi(xsys));

	if(l>=lBottom) {
		// if at finest level,

		// compute necessary vector differences (additional)
		EUCL_lincomb(xpos+(xA*dim),yPos[l]+(yB*dim),xAyB,1.,-1.,dim); // xA-yB

		// exact l.h.s. of shielding condition
		dist+=(getPhi(xAyB)-getPhi(xsyB));
		return (dist>=shieldingTolerance+slack);

	} else {

		// if at higher levels

		// compute necessary vector differences (additional)
		EUCL_lincomb(xpos+(xA*dim),xpos+(x*dim),xAxs,1.,-1.,dim); // xA-xs


		double xi,A,B,xsyB_Norm,xAxs_Norm,xAxs_xsyB_Angle;
		xsyB_Norm=sqrt(EUCL_innerProduct(xsyB,xsyB,dim));
		xAxs_Norm=sqrt(EUCL_innerProduct(xAxs,xAxs,dim));

		// compute xi (maximal angle distortion of xsyB by delta)
		if(yRadii[l][yB]<xsyB_Norm) {
			xi=asin(yRadii[l][yB]/xsyB_Norm);
		} else {
			xi=M_PI;
		}

		// compute angle between a and vB (set to 0, if one vector is zero)
		if((xAxs_Norm>0) && (xsyB_Norm>0)) {
			xAxs_xsyB_Angle=acos(EUCL_innerProduct(xAxs,xsyB,dim)/(xAxs_Norm*xsyB_Norm));
		} else {
			xAxs_xsyB_Angle=0.;
		}

		// compute factor A
		if(xAxs_xsyB_Angle+xi<M_PI) {
			A=xAxs_Norm*cos(xAxs_xsyB_Angle+xi);
		} else {
			A=-xAxs_Norm;
		}

		// compute factor B
		if(A>=0) {
			B=std::max(0.,xsyB_Norm-yRadii[l][yB]);
		} else {
			B=xsyB_Norm+yRadii[l][yB];
		}

		dist+=p*pow(B,p-1)*A;

		return (dist>shieldingTolerance+slack);

	}
}





/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////
/* Tree Benchmark Base Class */


TShieldGeneratorTreeBase_Benchmark::TShieldGeneratorTreeBase_Benchmark(int _dim,
		THierarchicalPartition* _yPartition, double** _yPos, double** _yRadii,
		int _lBottom, int _lTop,
		int *_xresH, double **_xposH, TVarListHandler **_xNeighboursH) :
		TShieldGeneratorTreeBase(_dim,
				_yPartition, _yPos, _yRadii,
				_lBottom, _lTop,
				_xresH, _xposH, _xNeighboursH) {
}

TShieldGeneratorTreeBase_Benchmark::~TShieldGeneratorTreeBase_Benchmark() {
}

void TShieldGeneratorTreeBase_Benchmark::generateShield(TVarListHandler* xVars,
		TVarListHandler *xSupport) {
	n_shielding_queries=0;
	TShieldGeneratorTreeBase::generateShield(xVars,xSupport);
}

bool TShieldGeneratorTreeBase_Benchmark::checkCondition(int xA, int l, int yB,
		int* xMap) {
	int nIndex,xn,yn;


	for(nIndex=0;nIndex<xNeighbours->lenList[xA];nIndex++) {
		// extract id of neighbour
		xn=xNeighbours->varList[xA][nIndex];
		yn=xMap[xn];
		n_shielding_queries++;
		if(checkConditionPlane(xA,xn,l,yB,yn)) {
			return true;
		}
	}

	return false;
}



template class TShieldGeneratorTree_SqrEuclideanPrototype<TShieldGeneratorTreeBase>;
template class TShieldGeneratorTree_SqrEuclideanPrototype<TShieldGeneratorTreeBase_Benchmark>;

template class TShieldGeneratorTree_SqrEuclideanNoisePrototype<TShieldGeneratorTreeBase>;
template class TShieldGeneratorTree_SqrEuclideanNoisePrototype<TShieldGeneratorTreeBase_Benchmark>;

template class TShieldGeneratorTree_PEuclideanPrototype<TShieldGeneratorTreeBase>;
template class TShieldGeneratorTree_PEuclideanPrototype<TShieldGeneratorTreeBase_Benchmark>;


